#include <mc_path_planner/graph_planner/a_star.h>


struct CompareNodes {
    bool operator()(const Node2DPtr& lhs, const Node2DPtr& rhs) const {
        return lhs->getF() > rhs->getF() || (lhs->getF() == rhs->getF() && lhs->getH() > rhs->getH());
    //     return lhs->getF() > rhs->getF() || 
    //         ((lhs->getF() >= rhs->getF() - 0.000001) 
    //             && (lhs->getF() <= rhs->getF() + 0.000001)
    //             && lhs->getH() > rhs->getH());
    }
};


AstarPlanner::AstarPlanner(boost::shared_ptr<MapBase> map_handler_ptr):
    PlannerBase(map_handler_ptr){ 
    debugMapPub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/grid_expend_map", 1, true);
    GLOG_INFO <<  "[AstarPlanner] init......";

    debug_ = true;
}

AstarPlanner::~AstarPlanner(){

}

// 路径规划
mc_status AstarPlanner::pathGenerate(Point2i start, Point2i goal, vector<Point2i>& points){
    mc_status status;
    points.clear();
    int width = mapHandlerPtr_->getWidth();
    int height = mapHandlerPtr_->getHeight();

    if(debug_){
        nav_msgs::OccupancyGrid::Ptr originMapPtr = mapHandlerPtr_->getOccupancyGrid();
        debugGrid_.header = originMapPtr->header;
        debugGrid_.info = originMapPtr->info;
        debugGrid_.data.resize(width * height);
        std::fill(debugGrid_.data.begin(), debugGrid_.data.end(), 0); 
    }

    Node2D nodeStart(start.x, start.y, 0, 0);
    Node2D nodeGoal(goal.x, goal.y, 0, 0);
    nodeStart.setIdx(width);
    nodeGoal.setIdx(width);
    nodeStart.updateH(nodeGoal);

    std::priority_queue<Node2DPtr, std::vector<Node2DPtr>, CompareNodes> openList;
    std::unordered_map<int, float> closedList;  //存储G值
    openList.push(std::make_shared<Node2D>(nodeStart));
    
    int iteration = 0;
    while(!ros::isShuttingDown() && !openList.empty()){
        // 获取优先级最高节点
        auto nodePred = openList.top();
        openList.pop();
        int idxPred = nodePred->getIdx();

        iteration++;

        if(debug_){
            int idx = (height - nodePred->getY()) * width + nodePred->getX();
            if(idx >= 0 && idx < debugGrid_.data.size())
                debugGrid_.data[idx] = 30;
        }

        // GLOG_INFO << "idxPred: " << idxPred 
        //     << ", x: " << nodePred->getX() 
        //     << ", y: " << nodePred->getY()
        //     << ", cost: " << nodePred->getF();

        if(closedList.find(idxPred) != closedList.end()){
            continue;
        }
        closedList[idxPred] = nodePred->getG();

        // 发现目标节点
        if(*nodePred == nodeGoal){
            GLOG_INFO << "find goal.";
            while(nodePred->getPred() != nullptr){
                points.emplace_back(Point2i(nodePred->getX(), nodePred->getY()));
                nodePred = nodePred->getPred();
            }  
            std::reverse(points.begin(), points.end());   
            GLOG_INFO << "points size: " << points.size();
            break;
        }

        // 遍历邻域节点
        for(int i = 0; i < Node2D::dir; i++) {
            int newX = nodePred->getX() + Node2D::dx[i];
            int newY = nodePred->getY() + Node2D::dy[i];
            auto nodeSucc = std::make_shared<Node2D>(newX, newY, nodePred->getG(), 0); 
            nodeSucc->setPred(nodePred);
            int idxSucc = nodeSucc->setIdx(width);

            // 障碍物检测 是否在闭列表
            if(!mapHandlerPtr_->ValueIsValid(Point2i(nodeSucc->getX(), nodeSucc->getY()))
                || closedList.find(idxSucc) != closedList.end()){
                continue;
            }

            // G值比较
            nodeSucc->updateG();
            float newG = nodeSucc->getG();
            if(closedList.find(idxSucc) == closedList.end() || newG < closedList[idxSucc]) {
                nodeSucc->updateH(nodeGoal);
                // GLOG_INFO << "--> i: " << i 
                //     << ", idxSucc: " << idxSucc 
                //     << ", x: " << nodeSucc->getX() 
                //     << ", y: " << nodeSucc->getY()
                //     << ", g: " << nodeSucc->getG()
                //     << ", h: " << nodeSucc->getH()
                //     << ", cost: " << nodeSucc->getF();
                openList.push(nodeSucc);
            }
                
        }
    }

    cout << "openList size: " << openList.size() 
        << ", closedList size: " << closedList.size()
        << ", iteration: " << iteration
        << std::endl;

    if(debug_){
        debugMapPub_.publish(debugGrid_);
    }
    return status;
}